#include <ros/ros.h>
#include "UWB_message_type.h"
#include <nlink_parser/LinktrackNodeframe0.h>
#include <nlink_parser/LinktrackNodeframe2.h>
#include <nlink_parser/LinktrackNodeframe3.h>
#include <nlink_parser/LinktrackNode0.h>
#include <std_msgs/String.h>
#include <eigen3/Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>




ros::Publisher pose_pub;
// ros::Publisher pose_raw_pub;


geometry_msgs::PoseStamped latest_pose;
uint32_t g_systemTime = 0;
double ros_time_minus_system_time = -UINT32_MAX/1000.0;



void UWB_frame0_callback(const nlink_parser::LinktrackNodeframe0::ConstPtr &msg)
{
    static tf::TransformBroadcaster br;

        for(int i = 0; i < msg->nodes.size();i++)
        {
            if(msg->nodes[i].data.size() == sizeof(UWB_messages::ObservationMessage))
            {
                UWB_messages::ObservationMessage obs_msg(UWB_messages::convertToMessage<UWB_messages::ObservationMessage>(msg->nodes[i].data));

                br.sendTransform(tf::StampedTransform(obs_msg.world_to_cam.to_tf_transform(), ros::Time::now(), "map", "cam" + std::to_string(msg->nodes[i].id)));
                br.sendTransform(tf::StampedTransform(obs_msg.observedState[0].pose.to_tf_transform(), ros::Time::now(), "cam" + std::to_string(msg->nodes[i].id), "base_link"));
                geometry_msgs::PoseStamped pose_new;
                pose_new.header.stamp = ros::Time::now();
                pose_new.pose = obs_msg.get_geometry_pose_world();
            }
        }

}


void UWB_frame3_callback(const nlink_parser::LinktrackNodeframe3::ConstPtr &msg)
{
    g_systemTime = msg->system_time;
    ros_time_minus_system_time = std::max(ros_time_minus_system_time, ros::Time::now().toSec() - g_systemTime/1000.0);
}

void UWB_frame2_callback(const nlink_parser::LinktrackNodeframe2::ConstPtr &msg)
{
    g_systemTime = msg->system_time;
    ros_time_minus_system_time = std::max(ros_time_minus_system_time, ros::Time::now().toSec() - g_systemTime/1000.0);
}

void pose_pub_timer_callback(const ros::TimerEvent&)
{
    pose_pub.publish(latest_pose);
}



int main(int argc, char *argv[])
{
    ros::init(argc, argv, "pnp_MV_cam");
    ros::NodeHandle nh;
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 1);
    // pose_raw_pub = nh.advertise<geometry_msgs::PoseStamped>("/cam_raw/pose", 1);
    ros::Subscriber UWB_frame0_sub = nh.subscribe<nlink_parser::LinktrackNodeframe0>("/nlink_linktrack_nodeframe0",1,UWB_frame0_callback);
    ros::Subscriber UWB_frame2_sub = nh.subscribe<nlink_parser::LinktrackNodeframe2>("/nlink_linktrack_nodeframe2",1,UWB_frame2_callback);
    ros::Subscriber UWB_frame3_sub = nh.subscribe<nlink_parser::LinktrackNodeframe3>("/nlink_linktrack_nodeframe3",1,UWB_frame3_callback);
    double pose_pub_rate;
    if(!ros::param::get("pose_pub_rate", pose_pub_rate))
    {
        ROS_ERROR("ros::param::get(\"pose_pub_rate\", pose_pub_rate) FAILED!!! ");
        return 0;
    }
    
    ros::spinOnce();
    ros::Timer pose_pub_timer = nh.createTimer(ros::Duration(1/pose_pub_rate), pose_pub_timer_callback);
    ros::spin();
    return 0;
}